
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       notify_board_led2.c
  * @author     baiyang
  * @date       2022-6-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "notify.h"
#include "notify_board_led2.h"

#include <assert.h>

#include <rtthread.h>
#include <rtdevice.h>
/*-----------------------------------macro------------------------------------*/
#if (defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN) && defined(RT_USING_PIN))

#ifndef HAL_GPIO_LED_ON
#define HAL_GPIO_LED_ON           0
#endif
#ifndef HAL_GPIO_LED_OFF
#define HAL_GPIO_LED_OFF          1
#endif

_Static_assert((HAL_GPIO_A_LED_PIN != HAL_GPIO_B_LED_PIN), "Duplicate LED assignments detected");

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void boardled2_ctor(board_led2_t boardled2);
static bool init(notify_device_t notify_dev);
static void update(notify_device_t notify_dev);
/*----------------------------------variable----------------------------------*/
static struct notify_device_ops board2_notify_dev_ops = {.NotifyDeviceDestructor = NULL,
                                                         .init                   = init,
                                                         .update                 = update,
                                                         .handle_led_control     = NULL,
                                                         .handle_play_tune       = NULL,
                                                         .play_tune              = NULL,
                                                         .rgb_control            = NULL,
                                                         .rgb_set_id             = NULL};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
notify_device_t boardled2_probe()
{
    board_led2_t boardled2 = (board_led2_t)rt_malloc(sizeof(struct board_led2));

    if (boardled2) {
        rt_memset(boardled2, 0, sizeof(struct board_led2));
        boardled2_ctor(boardled2);

        return (notify_device_t)boardled2;
    }

    return NULL;
}

static void boardled2_ctor(board_led2_t boardled2)
{
    boardled2->_counter = 0;
    boardled2->_sat_cnt=0;
    boardled2->save_trim_counter = 0;
    boardled2->arm_counter = 0;

    boardled2->notify_dev.ops = &board2_notify_dev_ops;
}

static bool init(notify_device_t notify_dev)
{
    // setup the main LEDs as outputs
    rt_pin_mode(HAL_GPIO_A_LED_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(HAL_GPIO_B_LED_PIN, PIN_MODE_OUTPUT);

    // turn all lights off
    rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
    rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
    return true;
}

/*
  main update function called at 50Hz
 */
static void update(notify_device_t notify_dev)
{
    board_led2_t boardled2 = (board_led2_t)notify_dev;

    boardled2->_counter++;

    // we never want to update LEDs at a higher than 16Hz rate
    if (boardled2->_counter % 3 != 0) {
        return;
    }

    // counter2 used to drop frequency down to 16hz
    uint8_t counter2 = boardled2->_counter / 3;

    // initialising
    if (notify_flags.initialising) {
        // blink LEDs A at 8Hz (full cycle) during initialisation
        rt_pin_write(HAL_GPIO_A_LED_PIN, (counter2 & 1) ? HAL_GPIO_LED_ON : HAL_GPIO_LED_OFF);
        return;
    }

    if ((counter2 & 0x2) == 0) {
        boardled2->save_trim_counter++;
    }

    bool led_a_used=false;

    // save trim and ESC calibration
    if (notify_flags.save_trim || notify_flags.esc_calibration) {
        switch(boardled2->save_trim_counter) {
            case 0:
                rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
                rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
                break;

            case 1:
                rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
                rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
                break;

            default:
                boardled2->save_trim_counter = -1;
                break;
        }
        return;
    }

    if(notify_flags.compass_cal_running ||
       notify_flags.temp_cal_running){
        // compass calibration or IMU temperature calibration
        switch(boardled2->save_trim_counter) {
        case 0:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short blinks by both LEDs
            rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
            break;
        case 1:
        case 2:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
            rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
            break;
        case 3:
        case 4:
        case 5:
        case 6:
        case 7:
            break;
        default:
            // reset counter to restart the sequence
            boardled2->save_trim_counter = -1;
            break;
        }
        return; 
        
    }

    if(notify_events.autotune_complete){
        switch(boardled2->save_trim_counter) {
        case 0:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short darkening
            break;
        case 1:
        case 2:
        case 3:
        case 4:
        case 5:
        case 6:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
            break;
        case 7:
            break;
        default:
            // reset counter to restart the sequence
            boardled2->save_trim_counter = -1;
            break;
        }
        led_a_used=true;
    }

    if(notify_events.autotune_failed){
        switch(boardled2->save_trim_counter) {
        case 0:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short double darkening
            break;
        case 1:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
            break;
        case 2:
        case 3:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); 
            break;
        case 4:
        case 5:
        case 6:
        case 7:
            rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
            break;
        default:
            // reset counter to restart the sequence
            boardled2->save_trim_counter = -1;
            break;
        }
        led_a_used=true;
    }

    // arming light
    if(!led_a_used) {
        if (notify_flags.armed) {
            if(notify_flags.failsafe_battery){//   blink slowly (around 2Hz)
                if ((counter2 & 0x7) == 0) {
                    rt_pin_write(HAL_GPIO_A_LED_PIN, (rt_base_t)!rt_pin_read(HAL_GPIO_A_LED_PIN));
                }
            }else if(notify_flags.failsafe_radio || notify_flags.failsafe_gcs){//   blink fast (around 4Hz)
                if ((counter2 & 0x3) == 0) {
                    rt_pin_write(HAL_GPIO_A_LED_PIN, (rt_base_t)!rt_pin_read(HAL_GPIO_A_LED_PIN));
                }
            } else {
                // ARM led solid
                rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
            }
        }else{
            if ((counter2 & 0x2) == 0) {
                boardled2->arm_counter++;
            }
        
            if (notify_flags.pre_arm_check) {
                // passed pre-arm checks so slower single flash
                switch(boardled2->arm_counter) {
                    case 0:
                    case 1:
                    case 2:
                    case 3:
                        rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
                        break;
                    case 4:
                    case 5:
                    case 6:
                    case 7:
                        rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
                        break;
                    default:
                        // reset counter to restart the sequence
                        boardled2->arm_counter = -1;
                        break;
                }
            }else{
                // failed pre-arm checks so double flash
                switch(boardled2->arm_counter) {
                    case 0:
                    case 1:
                        rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
                        break;
    
                    case 2:
                        rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
                        break;
                    
                    
                    case 3:
                    case 4:
                        rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
                        break;
    
                    case 5:
                    case 6:
                    case 7: // add one tick to do period be a multiple of the second
                        rt_pin_write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
                        break;
    
                    default:
                        boardled2->arm_counter = -1;
                        break;
                }
            }
        }
    }
    // gps light
    switch (notify_flags.gps_status) {
        case 0:
        case 1:
            // no GPS attached or no lock - be dark
            rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
            break;

        case 2: // 2d lock
        case 3://  3d lock
        default: // show number of sats 
            if ((counter2 & 0x2) == 0) {
                boardled2->_sat_cnt++;
            }
            uint16_t sats = notify_flags.gps_num_sats;
    
            if(boardled2->_sat_cnt<8) { // pause between pulses
                rt_pin_write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
            } else if(boardled2->_sat_cnt< (8 + (sats-6)*2) ) {
                rt_pin_write(HAL_GPIO_B_LED_PIN, (rt_base_t)!rt_pin_read(HAL_GPIO_B_LED_PIN)); // 2Hz
            } else {
                boardled2->_sat_cnt=-1;
            }            
            break;        
    }
}

/*------------------------------------test------------------------------------*/
#endif

